using System;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using System.Threading;
using Netduino4Fun.Framework;
using SecretLabs.NETMF.Hardware.NetduinoPlus;

namespace Netduino4Fun.Robot
{
    public class WheelController
    {
        const uint MaxSpeed = 50;
        const int Turn90Duration = 200;
        const int StepDuration = 500;

        readonly Motor leftMotor = new Motor(Pins.GPIO_PIN_D7, Pins.GPIO_PIN_D6);
        readonly Motor rightMotor = new Motor(Pins.GPIO_PIN_D4, Pins.GPIO_PIN_D5);


        private void SetRightMotorSpeed(int speed)
        {
            const uint correction = 0;

            uint motorSpeed = (uint)(MaxSpeed - speed);

            if (speed < 0 && motorSpeed >= correction)
                motorSpeed -= correction;
            
            rightMotor.Start(motorSpeed);
        }

        private void SetLeftMotorSpeed(int speed)
        {
            const uint correction = 0;

            uint motorSpeed = (uint)(MaxSpeed + speed);
            
            if (speed > 0 && motorSpeed >= correction)
                motorSpeed -= correction;
        
            leftMotor.Start(motorSpeed);
        }


        public void GoForward(uint speed)
        {
            SetLeftMotorSpeed((int)speed);
            SetRightMotorSpeed((int)speed);
        }

        public void GoBackward(uint speed)
        {
            SetLeftMotorSpeed((int)-speed);
            SetRightMotorSpeed((int)-speed);
        }

        public void TurnLeft(uint speed)
        {
            SetLeftMotorSpeed((int)-speed);
            SetRightMotorSpeed((int)speed);
        }

        public void TurnRight(uint speed)
        {
            SetLeftMotorSpeed((int)speed);
            SetRightMotorSpeed((int)-speed);
        }

        public void Turn90Left()
        {
            TurnLeft(50);
            Thread.Sleep(Turn90Duration);
            Stop();
        }

        public void Turn90Right()
        {
            TurnRight(50);
            Thread.Sleep(Turn90Duration);
            Stop();
        }

        public void StepForward()
        {
            GoForward(50);
            Thread.Sleep(StepDuration);
            Stop();
        }

        public void StepBackward()
        {
            GoBackward(50);
            Thread.Sleep(StepDuration);
            Stop();
        }

        public void Stop()
        {
            leftMotor.Stop();
            rightMotor.Stop();
        }


    }
}
